// --------------------------------------------------------------------------------------------------------------------
// <copyright file="KinematicChain.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------

namespace Microsoft.Robotics.Kinematics
{
    using System;
    using System.Collections.Generic;
    using System.Diagnostics;
    using System.Runtime.Serialization;

    using Microsoft.Robotics.Geometry;
    using Microsoft.Robotics.Numerics;

    using ShoNS.Array;

    /// <summary>
    /// A class to represent a kinematic chain.
    /// Joint list specifies all the joints and
    /// their kinematic relationship between links.
    /// </summary>
    [DataContract]
    public class KinematicChain
    {
        /// <summary>
        /// Initializes a new instance of the <see cref="KinematicChain" /> class.
        /// Joints describes the kinematic configuration of the chain for kinematics
        /// related computations.
        /// Use this constructor if only pure kinematic computations are needed.
        /// </summary>
        /// <param name="jointList">A list of joints for this kinematic chain</param>
        public KinematicChain(Joint[] jointList)
        {
            this.JointList = jointList;
        }

        /// <summary>
        /// Initializes a new instance of the <see cref="KinematicChain" /> class.
        /// Use this constructor if visualization of the geometry of the kinematic chain is needed.
        /// </summary>
        /// <param name="jointList">A list of joints for this kinematic chain</param>
        /// <param name="linkNames">A list of the names of the links</param>
        /// <param name="linkGeometryPaths">Path to the geometry of the links</param>
        /// <param name="endEffectorName">Name of the end effector</param>
        /// <param name="endEffectorGeometryPath">Path to the end effector</param>
        public KinematicChain(Joint[] jointList, string[] linkNames, string[] linkGeometryPaths, string endEffectorName, string endEffectorGeometryPath)
        {
            this.JointList = jointList;
            this.LinkNames = linkNames;
            this.LinkGeometryPaths = linkGeometryPaths;
            this.EndEffectorName = endEffectorName;
            this.EndEffectorGeometryPath = endEffectorGeometryPath;
        }

        /// <summary>
        /// Initializes a new instance of the <see cref="KinematicChain" /> class.
        /// Use this constructor if collision checking is needed.
        /// </summary>
        /// <param name="jointList">A list of joints for this kinematic chain</param>
        /// <param name="linkNames">A list of the names of the links</param>
        /// <param name="linkGeometryPaths">Path to the geometry of the links</param>
        /// <param name="endEffectorName">Name of the end effector</param>
        /// <param name="endEffectorGeometryPath">Path to the end effector</param>
        /// <param name="armCollisionModels">Collision model for the arm</param>
        /// <param name="endEffectorCollisionModel">Collision model for the end effector</param>
        public KinematicChain(Joint[] jointList, string[] linkNames, string[] linkGeometryPaths, string endEffectorName, string endEffectorGeometryPath, List<GeometryElementGroup3D> armCollisionModels, GeometryElementGroup3D endEffectorCollisionModel)
        {
            this.JointList = jointList;
            this.LinkNames = linkNames;
            this.LinkGeometryPaths = linkGeometryPaths;
            this.EndEffectorName = endEffectorName;
            this.EndEffectorGeometryPath = endEffectorGeometryPath;
            this.KinematicChainCollisionModelList = armCollisionModels;
            this.EndEffectorCollisionModel = endEffectorCollisionModel;
        }

        /// <summary>
        /// Gets the list of joints of this kinematic chain.
        /// </summary>
        [DataMember]
        public Joint[] JointList { get; private set; }

        /// <summary>
        /// Gets the list of the names of the links.
        /// </summary>
        [DataMember]
        public string[] LinkNames { get; private set; }

        /// <summary>
        /// Gets the path to the geometry
        /// </summary>
        [DataMember]
        public string[] LinkGeometryPaths { get; private set; }

        /// <summary>
        /// Gets the name of the end effector
        /// </summary>
        [DataMember]
        public string EndEffectorName { get; private set; }

        /// <summary>
        /// Gets the geometry path to the end effector
        /// </summary>
        [DataMember]
        public string EndEffectorGeometryPath { get; private set; }

        /// <summary>
        /// Gets the offset pose of the EE and in the last joint's coordinate system
        /// </summary>
        [DataMember]
        public Matrix4 EndEffectorOffsetInLastJoint { get; private set; }

        /// <summary>
        /// Gets the list of collision models of the kinematic chain
        /// Each link is defined with respect to its corresponding joint
        /// </summary>
        [DataMember]
        public List<GeometryElementGroup3D> KinematicChainCollisionModelList { get; private set; }

        /// <summary>
        /// Gets or sets the collision model of the end effector
        /// It is defined with respect to the origin of the end effector
        /// </summary>
        [DataMember]
        public GeometryElementGroup3D EndEffectorCollisionModel { get; set; }

        /// <summary>
        /// Gets or sets the collision model of the object in hand
        /// It is defined with respect to the origin of the end effector
        /// </summary>
        [DataMember]
        public GeometryElementGroup3D GraspedObjectCollisionModel { get; set; }

        /// <summary>
        /// Sets the transformation which specifies the EE in the last joint's coordinate system
        /// </summary>
        /// <param name="offsetTransformation">the transformation matrix</param>
        public void SetEndEffectorOffsetInLastJoint(Matrix4 offsetTransformation)
        {
            this.EndEffectorOffsetInLastJoint = offsetTransformation;
        }

        /// <summary>
        /// Sets the collision model of the grasped object in hand
        /// </summary>
        /// <param name="graspedObjectCollisionModel">The collision model of the object in hand</param>
        public void SetGraspedObjectCollisionModel(GeometryElementGroup3D graspedObjectCollisionModel)
        {
            this.GraspedObjectCollisionModel = graspedObjectCollisionModel;
        }

        /// <summary>
        /// get the values of all the joints
        /// </summary>
        /// <returns>the values of all the joints</returns>
        public double[] GetJointValues()
        {
            double[] jointValues = new double[this.JointList.Length];
            for (int i = 0; i < this.JointList.Length; ++i)
            {
                jointValues[i] = this.JointList[i].Value;
            }

            return jointValues;
        }

        /// <summary>
        /// Calculate the current pose of all the joints plus the end effector.
        /// </summary>
        /// <returns>an array of poses for all the joints</returns>
        public Matrix4[] GetJointTransforms()
        {
            Matrix4[] transformList = new Matrix4[this.JointList.Length + 1];

            // store the first joint's transform
            transformList[0] = this.JointList[0].GetOffsetTransform();

            // calculate every joint's transform based on their previous joint
            for (int i = 1; i < this.JointList.Length; ++i)
            {
                transformList[i] = transformList[i - 1] * this.JointList[i].GetOffsetTransform();
            }

            // calculate the end effector's transform
            transformList[this.JointList.Length] = transformList[this.JointList.Length - 1] * this.EndEffectorOffsetInLastJoint;

            return transformList;
        }

        /// <summary>
        /// Calculate the transformation matrix of joint i with respect to
        /// the base of this kinematic chain.
        /// </summary>
        /// <param name="index">the index of the joint</param>
        /// <returns>the transformation matrix of the joint</returns>
        public Matrix4 GetJointTransform(int index)
        {
            Matrix4 transform = Matrix4.Identity();

            for (int i = 0; i <= index; ++i)
            {
                transform = transform * this.JointList[i].GetOffsetTransform();
            }

            return transform;
        }

        /// <summary>
        /// Forward kinematics of this kinematic chain.
        /// Given an array of joint values, this function calculates the transformation
        /// matrices for each joint plus the end effector.
        /// </summary>
        /// <param name="jointValues">joint values in a column vector</param>
        /// <returns>an array of transformation matrices</returns>
        public Matrix4[] GetJointTransforms(DoubleArray jointValues)
        {
            Matrix4 transform = Matrix4.Identity();
            Matrix4[] result = new Matrix4[this.JointList.Length + 1];

            for (int i = 0; i < this.JointList.Length; ++i)
            {
                transform = transform * this.JointList[i].GetOffsetTransform(jointValues[i, 0]);
                result[i] = transform;
            }

            result[this.JointList.Length] = transform * this.EndEffectorOffsetInLastJoint;

            return result;
        }

        /// <summary>
        /// Calculate the EE's current pose.
        /// </summary>
        /// <returns>EE's current pose</returns>
        public Matrix4 GetEndEffectorTransform()
        {
            return this.GetJointTransform(this.JointList.Length - 1) * this.EndEffectorOffsetInLastJoint;
        }

        /// <summary>
        /// Calculate the EE's current pose.
        /// </summary>
        /// <param name="jointValues">joint values represented in a column vector</param>
        /// <returns>EE's current pose</returns>
        public Matrix4 GetEndEffectorTransform(DoubleArray jointValues)
        {
            return this.GetJointTransforms(jointValues)[(int)this.JointList.Length - 1] * this.EndEffectorOffsetInLastJoint;
        }

        /// <summary>
        /// Calculate the collision models of this robot at the given joint configuration
        /// The calculated model will include the arm itself, the end effector, and the object in the end effector
        /// </summary>
        /// <param name="jointValues">Joint values represented in a column vector</param>
        /// <param name="graspedObjectInEndEffectorTransform">Transformation of the grasped object in the end effector</param>
        /// <returns>The collision model</returns>
        public List<GeometryElementGroup3D> GetTransformedCollisionModels(DoubleArray jointValues, Matrix4 graspedObjectInEndEffectorTransform = null)
        {
            // compute the transforms for each joint of the kinematic chain
            Matrix4[] transforms = this.GetJointTransforms(jointValues);

            // have an assembly of the collision models for the entire arm
            List<GeometryElementGroup3D> collisionModel = new List<GeometryElementGroup3D>();

            // iterate through all the collision models of all the links
            if (null != this.KinematicChainCollisionModelList)
            {
                for (int i = 0; i < this.KinematicChainCollisionModelList.Count; ++i)
                {
                    // some links may not have a collision model associated
                    // a link's collision model may contain multiple geometry elements
                    if (null != this.KinematicChainCollisionModelList[i])
                    {
                        List<GeometryElement3D> group = this.KinematicChainCollisionModelList[i].ElementGroup;

                        // transform each geometry element to the correct position based on the joint
                        // transformations
                        for (int j = 0; j < this.KinematicChainCollisionModelList[i].ElementGroup.Count; ++j)
                        {
                            // sets the reference frame for the geometry element to the specific frame
                            // due to joint movement
                            group[j].ReferenceFrame = transforms[i];
                        }
                    }
                }

                collisionModel.AddRange(this.KinematicChainCollisionModelList);
            }

            // set the reference frame for the end effector and the object in hand
            if (null != this.EndEffectorCollisionModel)
            {
                for (int i = 0; i < this.EndEffectorCollisionModel.ElementGroup.Count; ++i)
                {
                    // the last transform matrix is the for the end effector
                    this.EndEffectorCollisionModel.ElementGroup[i].ReferenceFrame = transforms[transforms.Length - 1];
                }

                collisionModel.Add(this.EndEffectorCollisionModel);
            }

            // set the reference frame for the grasped object
            if (null != this.GraspedObjectCollisionModel && null != graspedObjectInEndEffectorTransform)
            {
                for (int i = 0; i < this.GraspedObjectCollisionModel.ElementGroup.Count; ++i)
                {
                    this.GraspedObjectCollisionModel.ElementGroup[i].ReferenceFrame = transforms[transforms.Length - 1] * graspedObjectInEndEffectorTransform;
                }

                collisionModel.Add(this.GraspedObjectCollisionModel);
            }

            return collisionModel;
        }

        /// <summary>
        /// Update all the joint values in this kinematic chain.
        /// </summary>
        /// <param name="values">Joint values to use</param>
        public void UpdateJointValues(DoubleArray values)
        {
            if (values.Length != this.JointList.Length)
            {
                string msg = string.Format("the length of the input values should match the number of joint of this kinematic chain, expected {0} while given {1}", this.JointList.Length, values.Length);
                throw new System.ArgumentException(msg);
            }

            for (int i = 0; i < this.JointList.Length; ++i)
            {
                this.JointList[i].Value = values[i, 0];
            }
        }

        /// <summary>
        /// Get the Jacobian matrix of this kinematic chain based on jointValues.
        /// Under-actuated kinematic chains are not considered here since we
        /// treat each joint independently.
        /// Reference frame is at the last joint of the kinematic chain.
        /// Refer to article "Differential Kinematic Control Equations for
        /// Simple Manipulators" by Richard Paul, Bruce Shimano, and Gordon Mayer
        /// for more details about the mathematics behind the calculation.
        /// </summary>
        /// <param name="jointValues">Current joint configuration of the kinematic chain represented in a column vector</param>
        /// <returns>The Jacobian matrix at current configuration specified by jointValues</returns>
        public DoubleArray GetLastJointJacobianMatrix(DoubleArray jointValues)
        {
            return this.GetJointJacobianMatrix(jointValues, this.JointList.Length - 1);
        }

        /// <summary>
        /// Get the Jacobian matrix of the joint specified by index using the given jointValues.
        /// The end effector can be considered by passing the index as the last joint index plus one.
        /// Under-actuated kinematic chains are not considered here since we treat each joint independently.
        /// Reference frame is at the specified joint.
        /// Refer to article "Differential Kinematic Control Equations for
        /// Simple Manipulators" by Richard Paul, Bruce Shimano, and Gordon Mayer
        /// for more details about the mathematics behind the calculation.
        /// </summary>
        /// <param name="jointValues">Current joint configuration of the kinematic chain represented in a column vector</param>
        /// <param name="jointIndex">The joint we want to compute a Jacobian matrix for</param>
        /// <returns>The Jacobian matrix at current configuration specified by jointValues</returns>
        public DoubleArray GetJointJacobianMatrix(DoubleArray jointValues, int jointIndex)
        {
            if (jointIndex > this.JointList.Length)
            {
                throw new System.IndexOutOfRangeException("Joint index out of range");
            }

            DoubleArray jacobian = new DoubleArray(6, this.JointList.Length);
            Matrix4[] transforms = this.GetJointTransforms(jointValues);

            // note the joint jointIndex is inclusive and is considered here
            // going through each joint before the one specified by jointIndex
            // collect all their influence
            for (int j = 0; j < this.JointList.Length; ++j)
            {
                // if the current joint is beyond what's queried for
                // there won't be any non-zero cells in the jacobian matrix
                if (j > jointIndex)
                {
                    jacobian[0, j] = 0;
                    jacobian[1, j] = 0;
                    jacobian[2, j] = 0;
                    jacobian[3, j] = 0;
                    jacobian[4, j] = 0;
                    jacobian[5, j] = 0;
                    continue;
                }

                Matrix4 transformLastJointInFramej = HomogenousTransform.Inverse(transforms[j]) * transforms[jointIndex];
                double px = transformLastJointInFramej[0, 3];
                double py = transformLastJointInFramej[1, 3];

                double nx = transformLastJointInFramej[0, 0];
                double ny = transformLastJointInFramej[1, 0];
                double nz = transformLastJointInFramej[2, 0];

                double ox = transformLastJointInFramej[0, 1];
                double oy = transformLastJointInFramej[1, 1];
                double oz = transformLastJointInFramej[2, 1];

                double ax = transformLastJointInFramej[0, 2];
                double ay = transformLastJointInFramej[1, 2];
                double az = transformLastJointInFramej[2, 2];

                if (JointType.REVOLUTE == this.JointList[j].Type())
                {
                    jacobian[0, j] = px * ny - py * nx;
                    jacobian[1, j] = px * oy - py * ox;
                    jacobian[2, j] = px * ay - py * ax;
                    jacobian[3, j] = nz;
                    jacobian[4, j] = oz;
                    jacobian[5, j] = az;
                }
                else if (JointType.PRISMATIC == this.JointList[j].Type())
                {
                    jacobian[0, j] = nz;
                    jacobian[1, j] = oz;
                    jacobian[2, j] = az;
                    jacobian[3, j] = jacobian[4, j] = jacobian[5, j] = 0;
                }
                else
                {
                    throw new System.InvalidOperationException("Cannot get joint Jacobian matrix due to invalid joint type");
                }
            }

            return jacobian;
        }

        /// <summary>
        /// Get the Jacobian matrix of the joint specified by index using the given jointValues.
        /// Reference frame is at the base of the robot (fixed frame). 
        /// The end effector can be considered by passing the index as the last joint index plus one.
        /// Under-actuated kinematic chains are not considered here since we treat each joint independently.
        /// Standard implementation for computing the geometric jacobian. Refer to a STANDARD ROBOTICS TEXTBOOK for details.
        /// </summary>
        /// <param name="jointValues">Current joint configuration of the kinematic chain represented in a column vector</param>
        /// <param name="jointIndex">The joint we want to compute a Jacobian matrix for</param>
        /// <returns>The Jacobian matrix at current configuration specified by jointValues</returns>
        public DoubleArray GetJointJacobianMatrixInWorldFrame(DoubleArray jointValues, int jointIndex)
        {
            // Ensure that we ask for the jacobian of an existing joint
            if (jointIndex > this.JointList.Length)
            {
                throw new System.IndexOutOfRangeException("Joint index out of range");
            }

            // Compute the FK transform for each joint based on the given configuration
            DoubleArray jacobian = new DoubleArray(6, this.JointList.Length);
            Matrix4[] transforms = this.GetJointTransforms(jointValues);

            // Get Homogeneous transform of joint "i" (T = [R t; 0 1])
            Matrix4 jointTransform_i = transforms[jointIndex];

            // Joint i's origin expressed in base frame
            Vector3 jointPosition_i = new Vector3(jointTransform_i[0, 3], jointTransform_i[1, 3], jointTransform_i[2, 3]);

            // Jacobian J relates linear & angular velocity to joint velocities
            // [v w] = J(q) qdot, where qdot is joint velocity vector

            // If we compute the jacobian for joint i, we have the following:
            // All joints with index j > i do not affect the linear & angular velocities at joint "j". So, the corresponding columns of the jacobians are all zero.
            // For joints with index j <= i, we have:
            // PRISMATIC JOINT:
            //  Contribution of that joint j's motion to linear velocity at joint "i" = z_j * qdot_j (z_j is z-axis of j^th joint in world frame. qdot_j is velocity at that joint (config space))
            //  Contribution of that joint j's motion to angular velocity at joint "i" = 0
            // Jac(:,j) = [z_j; 0];
            // REVOLUTE JOINT:
            //  Contribution of that joint j's rotation to linear velocity at joint "i" = (z_j * qdot_j) x (pos_i - pos_j) (this is the cross product of angular velocity vector & vector pointing from frame j to frame i's origin (V = R* W))
            //  Contribution of that joint j's rotation to angular velocity at joint "i" = (z_j * qdot_j)
            // Jac(:,j) = [z_j x (pos_i - pos_j); z_j]; 

            // note the joint jointIndex is inclusive and is considered here
            // going through each joint before the one specified by jointIndex
            // collect all their influence
            for (int j = 0; j < this.JointList.Length; ++j)
            {
                // if the current joint is beyond what's queried for
                // there won't be any non-zero cells in the jacobian matrix
                // DoubleArray initializes to zero, so we break here
                if (j > jointIndex)
                {
                    continue;
                }

                // Get the homogeneous transform of joint "j"
                Matrix4 jointTransform_j = transforms[j];

                if (JointType.REVOLUTE == this.JointList[j].Type())
                {
                    // Z-axis of joint "j"'s frame expressed in the base frame
                    Vector3 zaxis_j = new Vector3(jointTransform_j[0, 2], jointTransform_j[1, 2], jointTransform_j[2, 2]);

                    // Joint j's origin expressed in base frame
                    Vector3 jointPosition_j = new Vector3(jointTransform_j[0, 3], jointTransform_j[1, 3], jointTransform_j[2, 3]);

                    // Compute cross product between the vectors to get a vector proportional to linear velocity vector
                    // This is joint j's contribution to linear velocity at joint i
                    Vector3 jointLinearVelocity_j = Vector3.Cross(zaxis_j, (jointPosition_i - jointPosition_j));

                    // Set the first 3 elements of jacobian equal to this vector
                    jacobian[0, j] = jointLinearVelocity_j.X;
                    jacobian[1, j] = jointLinearVelocity_j.Y;
                    jacobian[2, j] = jointLinearVelocity_j.Z;

                    // Set the last 3 elements to be equal to z_j (angular velocity contributions)
                    jacobian[3, j] = zaxis_j.X;
                    jacobian[4, j] = zaxis_j.Y;
                    jacobian[5, j] = zaxis_j.Z;
                }
                else if (JointType.PRISMATIC == this.JointList[j].Type())
                {
                    // Z-axis of joint "j"'s frame expressed in the base frame
                    Vector3 zaxis_j = new Vector3(jointTransform_j[0, 2], jointTransform_j[1, 2], jointTransform_j[2, 2]);

                    // Set the FIRST 3 elements to be equal to z_j (linear velocity contributions)
                    jacobian[0, j] = zaxis_j.X;
                    jacobian[1, j] = zaxis_j.Y;
                    jacobian[2, j] = zaxis_j.Z;

                    // Angular velocity contribution is 0
                    jacobian[3, j] = jacobian[4, j] = jacobian[5, j] = 0;
                }
                else
                {
                    throw new System.InvalidOperationException("Cannot get joint Jacobian matrix due to invalid joint type");
                }
            }

            return jacobian;
        }

        /// <summary>
        /// Get the location of all the sphere centers and the position jacobian matrix of all the spheres on the robot at the configuration specified by the given jointValues.
        /// Reference frame is at the base of the robot (fixed frame). 
        /// Standard implementation for computing the geometric jacobian. Refer to a STANDARD ROBOTICS TEXTBOOK for details.
        /// The position jacobian is the 3 x NDOF part of the jacobian that only represents the change in position as the configuration changes.
        /// The implementation is ADHOC and intended for use primarily with the CHOMP motion planner and has been optimized for this.
        /// </summary>
        /// <param name="jointValues">Current joint configuration of the kinematic chain represented in a column vector</param>
        /// <param name="spherePosJacobians">Array of Position part of the jacobian matrix of all robot spheres. This is the output for this function. Memory has to be allocated apriori for this array. It has "K" 3 x NDOF matrices where "K" is the number of spheres in the robot model. </param>
        /// <param name="spherePositions">Array of locations all robot sphere centers in world frame. This is the output for this function. Memory has to be allocated apriori for this array. It has "K" 3 x NDOF matrices where "K" is the number of spheres in the robot model.</param>
        /// <param name="graspedObjectInEndEffectorTransform">Transformation of the grasped object in the end effector</param>
        public void GetSphereLocationAndPositionJacobianInWorldFrame(DoubleArray jointValues, DoubleArray[] spherePosJacobians, double[][] spherePositions, Matrix4 graspedObjectInEndEffectorTransform = null)
        {
            // Transform the robot model based on the current joint configuration
            // The transformed collision models do not have any "null" models in them. We need another way to identify the joint to which they belong to so that we get the correct jacobians.
            Matrix4[] transforms = this.GetJointTransforms(jointValues);

            // Jacobian J relates linear & angular velocity to joint velocities
            // [v w] = J(q) qdot, where qdot is joint velocity vector
            // If we compute the jacobian for joint i, we have the following:
            // All joints with index j > i do not affect the linear & angular velocities at joint "j". So, the corresponding columns of the jacobians are all zero.
            // For joints with index j <= i, we have:
            // PRISMATIC JOINT:
            //  Contribution of that joint j's motion to linear velocity at joint "i" = z_j * qdot_j (z_j is z-axis of j^th joint in world frame. qdot_j is velocity at that joint (config space))
            // Jac(:,j) = [z_j];
            // REVOLUTE JOINT:
            //  Contribution of that joint j's rotation to linear velocity at joint "i" = (z_j * qdot_j) x (pos_i - pos_j) (this is the cross product of angular velocity vector & vector pointing from frame j to frame i's origin (V = R* W))
            // Jac(:,j) = [z_j x (pos_i - pos_j)]; 

            // Iterate through the kinematic chain's collision model - Each model belongs to a link (or a joint). The FK transform is given by the transform of that particular joint.
            int ct = 0; // this is the count of the number of spheres seen till now
            for (int jointIndex = 0; jointIndex < this.KinematicChainCollisionModelList.Count; ++jointIndex)
            {
                // The original model may contain links that do not have any collision models
                if (null != this.KinematicChainCollisionModelList[jointIndex])
                {
                    // Get Homogeneous transform for that joint (T = [R t; 0 1])
                    Matrix4 jointTransform = transforms[jointIndex];

                    // iterate through all the elements within this link (leaving out all null links)
                    for (int i = 0; i < this.KinematicChainCollisionModelList[jointIndex].ElementGroup.Count; ++i)
                    {
                        // Ensure that memory has been allocated for the output
                        Debug.Assert(spherePosJacobians[ct].size0 == 3, "Output list 'sphereJacobians' does not have the correct memory (number of rows) allocated.");
                        Debug.Assert(spherePosJacobians[ct].size1 == this.JointList.Length, "Output list 'sphereJacobians' does not have the correct memory (number of columns) allocated.");
                        Debug.Assert(spherePositions[ct].Length == 3, "Output list 'spherePositions' does not have the correct memory allocated.");

                        // Get the corresponding sphere 
                        SphereElement a = (SphereElement)this.KinematicChainCollisionModelList[jointIndex].ElementGroup[i];

                        // Return the position of the sphere's center in the base frame to "spherePositions"
                        // Sphere "a" is located in link "i". So, we transform its position w.r.t link "i" into the global frame by using the transform for link "i"
                        // aPosInWorld = R * a + t;
                        spherePositions[ct][0] = jointTransform[0, 0] * a.Location.X + jointTransform[0, 1] * a.Location.Y + jointTransform[0, 2] * a.Location.Z + jointTransform[0, 3];
                        spherePositions[ct][1] = jointTransform[1, 0] * a.Location.X + jointTransform[1, 1] * a.Location.Y + jointTransform[1, 2] * a.Location.Z + jointTransform[1, 3];
                        spherePositions[ct][2] = jointTransform[2, 0] * a.Location.X + jointTransform[2, 1] * a.Location.Y + jointTransform[2, 2] * a.Location.Z + jointTransform[2, 3];

                        // Get the Jacobian matrix for that sphere and store it in spherePosJacobians[ct]
                        this.GetPointPositionJacobianInWorldFrame(spherePositions[ct], jointIndex, transforms, spherePosJacobians[ct]);

                        // Increment list counter by 1
                        ct++;
                    }
                }
            }

            // Next, iterate through all spheres in the end effector's collision model. The FK transform here is the end effector's transform (transforms[transforms.Length - 1])
            // The original model may contain links that do not have any collision models
            if (null != this.EndEffectorCollisionModel)
            {
                // End effector transform is the last one in the list of transforms
                int jointIndex = transforms.Length - 1;

                // Get Homogeneous transform of the end effector (T = [R t; 0 1])
                Matrix4 jointTransform = transforms[jointIndex];

                // iterate through all the elements within this link (leaving out all null links)
                for (int i = 0; i < this.EndEffectorCollisionModel.ElementGroup.Count; ++i)
                {
                    // Ensure that memory has been allocated for the output
                    Debug.Assert(spherePosJacobians[ct].size0 == 3, "Output list 'sphereJacobians' does not have the correct memory (number of rows) allocated.");
                    Debug.Assert(spherePosJacobians[ct].size1 == this.JointList.Length, "Output list 'sphereJacobians' does not have the correct memory (number of columns) allocated.");
                    Debug.Assert(spherePositions[ct].Length == 3, "Output list 'spherePositions' does not have the correct memory allocated.");

                    // Get the corresponding sphere 
                    SphereElement a = (SphereElement)this.EndEffectorCollisionModel.ElementGroup[i];

                    // Return the position of the sphere's center in the base frame to "spherePositions"
                    // Sphere "a" is located in link "i". So, we transform its position w.r.t link "i" into the global frame by using the transform for link "i"
                    // aPosInWorld = R * a + t;
                    spherePositions[ct][0] = jointTransform[0, 0] * a.Location.X + jointTransform[0, 1] * a.Location.Y + jointTransform[0, 2] * a.Location.Z + jointTransform[0, 3];
                    spherePositions[ct][1] = jointTransform[1, 0] * a.Location.X + jointTransform[1, 1] * a.Location.Y + jointTransform[1, 2] * a.Location.Z + jointTransform[1, 3];
                    spherePositions[ct][2] = jointTransform[2, 0] * a.Location.X + jointTransform[2, 1] * a.Location.Y + jointTransform[2, 2] * a.Location.Z + jointTransform[2, 3];

                    // Get the Jacobian matrix for that sphere and store it in spherePosJacobians[ct]
                    this.GetPointPositionJacobianInWorldFrame(spherePositions[ct], jointIndex, transforms, spherePosJacobians[ct]);

                    // Increment list counter by 1
                    ct++;
                }
            }

            // Next, iterate through all spheres in the end effector's collision model. The FK transform here is the end effector's transform (transforms[transforms.Length - 1])
            // The original model may contain links that do not have any collision models
            // set the reference frame for the grasped object
            if (null != this.GraspedObjectCollisionModel && null != graspedObjectInEndEffectorTransform)
            {
                // We can assume that the index is the same as the end effector index
                int jointIndex = transforms.Length - 1;

                // Get Homogeneous transform for the grasped object (we take into account the relative transform from the end effector as well here) (T = [R t; 0 1])
                Matrix4 jointTransform = transforms[jointIndex] * graspedObjectInEndEffectorTransform;

                // iterate through all the elements within this link (leaving out all null links)
                for (int i = 0; i < this.GraspedObjectCollisionModel.ElementGroup.Count; ++i)
                {
                    // Ensure that memory has been allocated for the output
                    Debug.Assert(spherePosJacobians[ct].size0 == 3, "Output list 'sphereJacobians' does not have the correct memory (number of rows) allocated.");
                    Debug.Assert(spherePosJacobians[ct].size1 == this.JointList.Length, "Output list 'sphereJacobians' does not have the correct memory (number of columns) allocated.");
                    Debug.Assert(spherePositions[ct].Length == 3, "Output list 'spherePositions' does not have the correct memory allocated.");

                    // Get the corresponding sphere 
                    SphereElement a = (SphereElement)this.GraspedObjectCollisionModel.ElementGroup[i];

                    // Return the position of the sphere's center in the base frame to "spherePositions"
                    // Sphere "a" is located in link "i". So, we transform its position w.r.t link "i" into the global frame by using the transform for link "i"
                    // aPosInWorld = R * a + t;
                    spherePositions[ct][0] = jointTransform[0, 0] * a.Location.X + jointTransform[0, 1] * a.Location.Y + jointTransform[0, 2] * a.Location.Z + jointTransform[0, 3];
                    spherePositions[ct][1] = jointTransform[1, 0] * a.Location.X + jointTransform[1, 1] * a.Location.Y + jointTransform[1, 2] * a.Location.Z + jointTransform[1, 3];
                    spherePositions[ct][2] = jointTransform[2, 0] * a.Location.X + jointTransform[2, 1] * a.Location.Y + jointTransform[2, 2] * a.Location.Z + jointTransform[2, 3];

                    // Get the Jacobian matrix for that sphere and store it in spherePosJacobians[ct]
                    this.GetPointPositionJacobianInWorldFrame(spherePositions[ct], jointIndex, transforms, spherePosJacobians[ct]);

                    // Increment list counter by 1
                    ct++;
                }
            }
        }

        /// <summary>
        /// Get the position jacobian (linear velocity part) for a point on the kinematic chain, given the point's co-ordinates in base frame, the index of the joint to which the point belongs,
        /// the forward kinematic transforms for all the joints in the kinematic chain and an array to store the result 
        /// </summary>
        /// <param name="point"> 3D coordinates of the point in the base frame of the kinematic chain </param>
        /// <param name="jointIndex"> Index of the joint to which the point belongs to. This could be greater than the number of joints we can control (for example, the end effector) </param>
        /// <param name="transforms"> Forward Kinematic transforms for all the joints in the kinematic chain </param>
        /// <param name="posJacobian"> Array to store the final result. This is a 3 x Number of DOF array whose memory has been pre-allocated. </param>
        public void GetPointPositionJacobianInWorldFrame(double[] point, int jointIndex, Matrix4[] transforms, DoubleArray posJacobian)
        {
            // note the joint jointIndex is inclusive and is considered here
            // going through each joint before the one specified by jointIndex
            for (int j = 0; j < this.JointList.Length; ++j)
            {
                // if the current joint is beyond the index of the joint to which we belong,
                // there won't be any non-zero cells in the jacobian matrix (this is exclusive of the current joint)
                if (j > jointIndex)
                {
                    posJacobian[0, j] = 0;
                    posJacobian[1, j] = 0;
                    posJacobian[2, j] = 0;
                    continue;
                }

                // Get the homogeneous transform of joint "j" 
                Matrix4 jointTransform = transforms[j];

                if (JointType.REVOLUTE == this.JointList[j].Type())
                {
                    // Vector pointing from center of joint "j" to the point [dx; dy; dz]
                    double dx = point[0] - jointTransform[0, 3]; // x difference
                    double dy = point[1] - jointTransform[1, 3]; // y difference
                    double dz = point[2] - jointTransform[2, 3]; // z difference

                    // Z-axis of joint "j"'s frame expressed in the base frame
                    double nx = jointTransform[0, 2];
                    double ny = jointTransform[1, 2];
                    double nz = jointTransform[2, 2];

                    // Compute cross product between the vectors to get a vector proportional to linear velocity vector
                    // This is joint j's contribution to linear velocity at joint i
                    // linVel = z_j x (pos_i - pos_j) where z_j is z direction of joint "j" 
                    // (pos_i - pos_j) is the vector from joint j's origin to out point
                    posJacobian[0, j] = ny * dz - nz * dy;
                    posJacobian[1, j] = nz * dx - nx * dz;
                    posJacobian[2, j] = nx * dy - ny * dx;
                }
                else if (JointType.PRISMATIC == this.JointList[j].Type())
                {
                    // Set the FIRST 3 elements to be equal to Z-axis of joint "j"'s frame expressed in the base frame (linear velocity contributions)
                    posJacobian[0, j] = jointTransform[0, 2];
                    posJacobian[1, j] = jointTransform[1, 2];
                    posJacobian[2, j] = jointTransform[2, 2];
                }
                else
                {
                    throw new System.InvalidOperationException("Cannot get joint Jacobian matrix due to invalid joint type");
                }
            }
        }
    }
}
